#include "Grab.h"
static void Grab_Init();                            // 初始化函数
static void Pos_Init();                             // 初始化姿态
static void Grab_Item(ServoMove moves[], int size); // 抓取循环
static void moveServo(Servo servo, int startPos, int endPos);
Servo base, rArm, fArm, claw;

// 创建结构体数组，用于存储三次抓取的舵机移动信息
ServoMove moves_1[] = {
    {claw, 90, 5},
    {base, 90, 110},
    {fArm, 40, 140},
    {rArm, 90, 138},
    {claw, 5, 120},
    {rArm, 138, 90},
    {base, 115, 175},
    {fArm, 140, 130},
    {rArm, 90, 128},
    {claw, 120, 5},
    {rArm, 128, 90}};

ServoMove moves_2[] = {
    {claw, 90, 5},
    {base, 90, 78},
    {fArm, 40, 140},
    {rArm, 90, 148},
    {claw, 5, 120},
    {rArm, 148, 90},
    {base, 78, 155},
    // {fArm, 140, 120},
    {rArm, 90, 128},
    {claw, 120, 5},
    {rArm, 128, 90}};

ServoMove moves_3[] = {
    {claw, 90, 5},
    {base, 90, 60},
    {fArm, 40, 150},
    {rArm, 90, 138},
    {claw, 5, 120},
    {rArm, 148, 90},
    {base, 60, 142},
    {fArm, 150, 170},
    {rArm, 90, 158},
    {claw, 120, 5},
    {rArm, 158, 90}};

// Grab的主函数
void Grab()
{
    Grab_Init();
    Grab_Item(moves_1, sizeof(moves_1) / sizeof(moves_1[0]));
    Grab_Item(moves_2, sizeof(moves_2) / sizeof(moves_2[0]));
    Grab_Item(moves_3, sizeof(moves_3) / sizeof(moves_3[0]));
}

static void Grab_Init()
{
    // 配置引脚，准备抓取
    base.attach(11);
    rArm.attach(5);
    fArm.attach(3);
    claw.attach(6);
    Serial.begin(9600);

    Pos_Init();
}

/*
* 模块名:通用的舵机移动函数
* 代码描述:int pos = startPos;：首先，初始化 pos 为 startPos，这是伺服电机的起始位置。
startPos < endPos ? pos < endPos : pos > endPos;：这是循环的条件。如果 startPos 小于 endPos，那么当 pos 小于 endPos 时，循环继续；否则，当 pos 大于 endPos 时，循环继续。这意味着伺服电机可以向前或向后移动。
pos += step：在每次循环中，pos 会增加 step 的值。step 的值根据 startPos 和 endPos 的大小关系确定，如果 startPos 小于 endPos，step 为 1，否则 step 为 -1。这样，伺服电机可以逐步向前或向后移动。
这个 for 循环的目的是使伺服电机逐步从 startPos 移动到 endPos，无论 startPos 是大于还是小于 endPos。
* 作者:
* 创建时间:2024/05/17 11:31:44
*/
static void moveServo(Servo servo, int startPos, int endPos)
{
    int step = startPos < endPos ? 1 : -1;
    for (int pos = startPos; startPos < endPos ? pos < endPos : pos > endPos; pos += step)
    {
        servo.write(pos);
        delay(10);
    }
}
// 姿态初始化函数
static void Pos_Init()
{
    base.write(90);
    delay(500);
    rArm.write(90);
    delay(500);
    fArm.write(40);
    delay(500);
    claw.write(90);
    delay(500);
}

static void Grab_Item(ServoMove moves[], int size)
{
    // 遍历执行此次抓取的舵机行为
    for (int i = 0; i < size; i++)
    {
        moveServo(moves[i].servo, moves[i].startPos, moves[i].endPos);
    }
    Pos_Init();
}